/**
  *****************************************************************************
  * @Author       : JackyJuu
  * @Site         : https://www.geekros.com
  * @LastEditTime : 2022-03-18 17:59:15
  * @FilePath     : \Geek_Dog\hardware\robot\lite_dog\src\Dog_app.c
  * @Description  : 机器狗应用层相关代码
  * @Copyright (c) 2022 by GEEKROS, All Rights Reserved. 
  ******************************************************************************
*/

#include "math.h"
#include "servo.h"
#include "dog_app.h"
#include "lite_dog.h"
#include "flash.h"

extern Lite_Dog_Struct Lite_Dog;


/*******************************************************************************
  * @funtion      : Get_XY_Coordinate
  * @LastEditors  : JackyJuu
  * @description  : 根据大小腿角度解算xy坐标系
  * @param         {float} Leg_A_Angle 大腿与身体夹角角度
  * @param         {float} Leg_B_Angle 小腿与大腿夹角角度
  * @param         {float*} Get_X_Codi 解算出的X轴坐标系数据地址
  * @param         {float*} Get_Y_Codi 解算出的Y轴坐标系数据地址
  * @return        {*}
  *******************************************************************************/
void Get_XY_Coordinate(float Leg_A_Angle,float Leg_B_Angle,float* Get_X_Codi,float* Get_Y_Codi)
{
	float Leg_A_Length,Leg_B_Length;
	float A_Angle_t,B_Angle_t,A_Angle,B_Angle,AB_Angle; 
	float Get_X,Get_Y;  

	Leg_A_Length = Lite_Dog.Dog_Config_Data.thigh_length;
	Leg_B_Length = Lite_Dog.Dog_Config_Data.calf_length;

	A_Angle_t = Leg_A_Angle;
	B_Angle_t = Leg_B_Angle;

	A_Angle = Leg_A_Angle/180 * PI;
	B_Angle = Leg_B_Angle/180 * PI;

	float Test_C,Test_Angle;
	Test_C = sqrt(pow(Leg_A_Length,2) + pow(Leg_B_Length,2) - 2 * Leg_A_Length * Leg_B_Length * cos(B_Angle));
	Test_Angle = acos((pow(Leg_A_Length,2) + pow(Test_C,2) - pow(Leg_A_Length,2)) / (2 * Leg_A_Length * Test_C));

	Get_X = Test_C * cos(-A_Angle + Test_Angle);
	Get_Y = Test_C * sin(-A_Angle + Test_Angle);

	*Get_X_Codi = Get_X;
	*Get_Y_Codi = Get_Y;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Angle_To_Coordinate
  * @LastEditors  : JackyJuu
  * @description  : 解算舵机位置值，获取大腿角度，解算足端坐标
  * @param         {Lite_Dog_Struct*} Dog_Angle_To_Coordinate 狗的数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Angle_To_Coordinate(Lite_Dog_Struct* Dog_Angle_To_Coordinate)
{
	Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Angle_Now[0] = (Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Offset_Data[0] - (float)Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Motor_Message[0]->position) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Angle_Now[1] = 180 - ((float)Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Motor_Message[1]->position - Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Offset_Data[1]) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Angle_Now[2] = Dog_C_Angle_Min + ((float)Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Motor_Message[1]->position - (float)Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Motor_Message[2]->position - Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Offset_Data[2])  * SERVO_SCS_DATA_CHANGE * Dog_C_Leg_Anlge_Count;

	Get_XY_Coordinate(Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Angle_Now[1],Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Leg_Angle_Now[2],\
					 &Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Now_Leg_Loc[1],&Dog_Angle_To_Coordinate->Dog_LF_Leg_Data.Now_Leg_Loc[2]);

	Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Angle_Now[0] = ((float)Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Motor_Message[0]->position - Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Offset_Data[0]) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Angle_Now[1] = 180 - (Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Offset_Data[1] - (float)Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Motor_Message[1]->position) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Angle_Now[2] = Dog_C_Angle_Min + (Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Offset_Data[2] - ((float)Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Motor_Message[1]->position - (float)Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Motor_Message[2]->position))  * SERVO_SCS_DATA_CHANGE * Dog_C_Leg_Anlge_Count;

	Get_XY_Coordinate(Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Angle_Now[1],Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Leg_Angle_Now[2],\
					  &Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Now_Leg_Loc[1],&Dog_Angle_To_Coordinate->Dog_RF_Leg_Data.Now_Leg_Loc[2]);

	Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Angle_Now[0] = ((float)Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Motor_Message[0]->position - Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Offset_Data[0]) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Angle_Now[1] = 180 - ((float)Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Motor_Message[1]->position - Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Offset_Data[1]) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Angle_Now[2] = Dog_C_Angle_Min + ((float)Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Motor_Message[1]->position - (float)Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Motor_Message[2]->position - Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Offset_Data[2])  * SERVO_SCS_DATA_CHANGE * Dog_C_Leg_Anlge_Count;

	Get_XY_Coordinate(Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Angle_Now[1],Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Leg_Angle_Now[2],\
					  &Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Now_Leg_Loc[1],&Dog_Angle_To_Coordinate->Dog_LB_Leg_Data.Now_Leg_Loc[2]);

	Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Angle_Now[0] = (Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Offset_Data[0] - (float)Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Motor_Message[0]->position) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Angle_Now[1] = 180 - (Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Offset_Data[1] - (float)Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Motor_Message[1]->position) * SERVO_SCS_DATA_CHANGE;
	Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Angle_Now[2] = Dog_C_Angle_Min + (Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Offset_Data[2] - ((float)Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Motor_Message[1]->position - (float)Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Motor_Message[2]->position))  * SERVO_SCS_DATA_CHANGE * Dog_C_Leg_Anlge_Count;

	Get_XY_Coordinate(Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Angle_Now[1],Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Leg_Angle_Now[2],\
					  &Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Now_Leg_Loc[1],&Dog_Angle_To_Coordinate->Dog_RB_Leg_Data.Now_Leg_Loc[2]);

}


/*******************************************************************************
  * @funtion      : Get_Dog_Legs_Angle  
  * @LastEditors  : JackyJuu
  * @description  : 根据坐标值解算大小腿角度
  * @param         {float} Set_X_Data 足端坐标值X轴值
  * @param         {float} Set_Y_Data 足端坐标值Y轴值
  * @param         {float*} Get_A_Angle_t 获取大腿角度数据地址
  * @param         {float*} Get_B_Angle_t 获取小腿角度数据地址
  * @return        {*}
  *******************************************************************************/
void Get_Dog_Legs_Angle(float Set_X_Data,float Set_Y_Data,float* Get_A_Angle_t,float* Get_B_Angle_t)
{

	float Dog_Leg_A_Length,Dog_Leg_B_Length;
	float Get_B_Angle,Get_A_Angle;  
	float Cal_Data_B,Cal_Data_A1,Get_A1_Angle,Get_A2_Angle;

	Dog_Leg_A_Length = Lite_Dog.Dog_Config_Data.thigh_length;
	Dog_Leg_B_Length = Lite_Dog.Dog_Config_Data.calf_length;

	Cal_Data_B = ((Dog_Leg_A_Length *Dog_Leg_A_Length + Dog_Leg_B_Length* Dog_Leg_B_Length -Set_X_Data * Set_X_Data - Set_Y_Data * Set_Y_Data)/(2 * Dog_Leg_A_Length * Dog_Leg_B_Length));
	Get_B_Angle = acos(Cal_Data_B);
	
	Cal_Data_A1 = (( Dog_Leg_A_Length * Dog_Leg_A_Length + Set_X_Data * Set_X_Data + Set_Y_Data * Set_Y_Data - Dog_Leg_B_Length * Dog_Leg_B_Length)/(2 * Dog_Leg_A_Length * sqrt(Set_X_Data * Set_X_Data + Set_Y_Data * Set_Y_Data)));
	Get_A1_Angle = acos(Cal_Data_A1);
	Get_A2_Angle = atan(Set_Y_Data/Set_X_Data);

	if(Get_A2_Angle > 0)
	{
		Get_A2_Angle = Get_A2_Angle - PI;
	}

	Get_A_Angle= Get_A1_Angle - Get_A2_Angle;
	*Get_A_Angle_t = Get_A_Angle;
	*Get_B_Angle_t = Get_B_Angle;
}


/*******************************************************************************
  * @funtion      : Lite_Dog_Servo_To_Data  
  * @LastEditors  : JackyJuu
  * @description  : 用于获取舵机数据，每隔1s获取一次，此函数可注释不影响运行
  * @param         {Lite_Dog_Struct*} Dog_Servo_To_Leg 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Servo_To_Data(Lite_Dog_Struct* Dog_Servo_To_Leg)
{
	static int Count_Num = 0;
	if(Count_Num * Dog_Delay_Time_Set % 1000 == 1)
	{
		LOS_TaskLock();
		Single_Leg_Get_Servo_Data(&Dog_Servo_To_Leg->Dog_LF_Leg_Data);
		Single_Leg_Get_Servo_Data(&Dog_Servo_To_Leg->Dog_RF_Leg_Data);
		Single_Leg_Get_Servo_Data(&Dog_Servo_To_Leg->Dog_LB_Leg_Data);
		Single_Leg_Get_Servo_Data(&Dog_Servo_To_Leg->Dog_RB_Leg_Data);
		LOS_TaskUnlock();	
		Lite_Dog_Angle_To_Coordinate(Dog_Servo_To_Leg);
		Count_Num = 0;
	}
		Count_Num++;
}

void Lite_Dog_Coodinate_Check(Lite_Dog_Struct* Dog_Coordinate_To_Angle,float* X,float* Y,float* Z)
{

}


/*******************************************************************************
  * @funtion      : Lite_Dog_Coordinate_To_Angle  
  * @LastEditors  : JackyJuu
  * @description  : 根据坐标系解算机器狗腿部角度，并解算为舵机实际位置值
  * @param         {Lite_Dog_Struct*} All_Legs_Angle_Data 传递机器狗数据结构体地址
  * @param         {float*} X 设置机器狗四足坐标系X轴数组地址
  * @param         {float*} Y 设置机器狗四足坐标系Y轴数组地址
  * @param         {float*} Z 设置机器狗四足坐标系Y轴数组地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Coordinate_To_Angle(Lite_Dog_Struct* Dog_Coordinate_To_Angle,float* X,float* Y,float* Z)
{

	Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Set_Leg_Loc[0] = X[0];
	Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Set_Leg_Loc[1] = Y[0];
	Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Set_Leg_Loc[2] = Z[0];

	Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Set_Leg_Loc[0] = X[1];
	Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Set_Leg_Loc[1] = Y[1];
	Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Set_Leg_Loc[2] = Z[1];

	Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Set_Leg_Loc[0] = X[2];
	Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Set_Leg_Loc[1] = Y[2];
	Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Set_Leg_Loc[2] = Z[2];

	Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Set_Leg_Loc[0] = X[3];
	Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Set_Leg_Loc[1] = Y[3];
	Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Set_Leg_Loc[2] = Z[3];

	Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Leg_Angle_Set[0] = 0;
	Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Leg_Angle_Set[0] = 0;
	Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Leg_Angle_Set[0] = 0;
	Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Leg_Angle_Set[0] = 0;

	Get_Dog_Legs_Angle(Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Set_Leg_Loc[1],Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Set_Leg_Loc[2],\
	&Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Leg_Angle_Set[1],&Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Leg_Angle_Set[2]);
	Get_Dog_Legs_Angle(Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Set_Leg_Loc[1],Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Set_Leg_Loc[2],\
	&Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Leg_Angle_Set[1],&Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Leg_Angle_Set[2]);
	Get_Dog_Legs_Angle(Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Set_Leg_Loc[1],Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Set_Leg_Loc[2],\
	&Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Leg_Angle_Set[1],&Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Leg_Angle_Set[2]);
	Get_Dog_Legs_Angle(Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Set_Leg_Loc[1],Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Set_Leg_Loc[2],\
	&Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Leg_Angle_Set[1],&Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Leg_Angle_Set[2]);
	
	Dog_Coordinate_To_Angle->leg_angle.butt[0] = Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Leg_Angle_Set[0]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.butt[1] = Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Leg_Angle_Set[0]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.butt[2] = Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Leg_Angle_Set[0]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.butt[3] = Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Leg_Angle_Set[0]/PI*180;

	Dog_Coordinate_To_Angle->leg_angle.thigh[0] = Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Leg_Angle_Set[1]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.thigh[1] = Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Leg_Angle_Set[1]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.thigh[2] = Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Leg_Angle_Set[1]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.thigh[3] = Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Leg_Angle_Set[1]/PI*180;

	Dog_Coordinate_To_Angle->leg_angle.calf[0] = Dog_Coordinate_To_Angle->Dog_LF_Leg_Data.Leg_Angle_Set[2]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.calf[1] = Dog_Coordinate_To_Angle->Dog_RF_Leg_Data.Leg_Angle_Set[2]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.calf[2] = Dog_Coordinate_To_Angle->Dog_LB_Leg_Data.Leg_Angle_Set[2]/PI*180;
	Dog_Coordinate_To_Angle->leg_angle.calf[3] = Dog_Coordinate_To_Angle->Dog_RB_Leg_Data.Leg_Angle_Set[2]/PI*180;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Data_To_Servo  
  * @LastEditors  : JackyJuu
  * @description  : 获取腿部角度数据转化为舵机数据发送至舵机
  * @param         {Lite_Dog_Struct*} Dog_Data_To_Servo 获取机器狗数据结构地址
  * @param         {uint16_t} Time 设置移动时间，0为最大速度
  * @param         {uint16_t} Speed 设置移动速度，0为最大速度
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Data_To_Servo(Lite_Dog_Struct* Dog_Data_To_Servo, uint16_t Time, uint16_t Speed)
{
	uint8_t  Servo_ID[12];
	uint16_t Servo_Position[12];
	uint16_t Servo_Time[12];
	uint16_t Servo_Speed[12];

	int servo_num = 12;

	Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[0] = Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[1] = Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Offset_Data[1] + (180 - (Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
	Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[2] = Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[1] - (Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Offset_Data[2] + ((Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));

	Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[0] = Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[1] = Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Offset_Data[1] - (180 - (Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
	Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[2] = Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[1] - (Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Offset_Data[2] - ((Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));

	Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[0] = Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[1] = Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Offset_Data[1] + (180 - (Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
	Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[2] = Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[1] - (Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Offset_Data[2] + ((Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));

	Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[0] = Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[1] = Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Offset_Data[1] - (180 - (Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
	Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[2] = Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[1] - (Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Offset_Data[2] - ((Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));

	Servo_Position[0] = Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[0];
	Servo_Position[1] = Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[0];
	Servo_Position[2] = Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[0];
	Servo_Position[3] = Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[0];

	Servo_Position[4] = Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[1];
	Servo_Position[5] = Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[1];
	Servo_Position[6] = Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[1];
	Servo_Position[7] = Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[1];

	Servo_Position[8] = Dog_Data_To_Servo->Dog_LF_Leg_Data.Leg_Servo_Pos_Set[2];
	Servo_Position[9] = Dog_Data_To_Servo->Dog_RF_Leg_Data.Leg_Servo_Pos_Set[2];
	Servo_Position[10] = Dog_Data_To_Servo->Dog_LB_Leg_Data.Leg_Servo_Pos_Set[2];
	Servo_Position[11] = Dog_Data_To_Servo->Dog_RB_Leg_Data.Leg_Servo_Pos_Set[2];

	for(int i = 0;i < 12;i++)
	{
		Servo_ID[i] = i+1;
		Servo_Time[i] = Time;
		Servo_Speed[i] = Speed;
	}

	// 发送舵机位置控制数据
	Servo_Sync_Write_Position(Servo_ID, servo_num, Servo_Position, Servo_Time, Servo_Speed);
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Attitude_Update  
  * @LastEditors  : JackyJuu
  * @description  : 更新机器狗当前状态
  * @param         {Dog_State_Data_Struct} *Dog_Attitude_Update 传递用于存储机器狗状态的地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Attitude_Update(Dog_State_Data_Struct *Dog_Attitude_Update)
{
	
	float Dog_Length = 157.0;
	float Dog_Width = 108.0f;

    Dog_Attitude_Update->Dog_Now_Height = (float)(Dog_Attitude_Update->Dog_Body_Now_Cood[0][1] + \
    Dog_Attitude_Update->Dog_Body_Now_Cood[1][1] + Dog_Attitude_Update->Dog_Body_Now_Cood[2][1] + \
    Dog_Attitude_Update->Dog_Body_Now_Cood[3][1])/4;

    Dog_Attitude_Update->Dog_Now_Gravity = (float)(Dog_Attitude_Update->Dog_Body_Now_Cood[0][0] + \
    Dog_Attitude_Update->Dog_Body_Now_Cood[1][0] + Dog_Attitude_Update->Dog_Body_Now_Cood[2][0] + \
    Dog_Attitude_Update->Dog_Body_Now_Cood[3][0])/4;

    Dog_Attitude_Update->Dog_Now_Pitch = Dog_Attitude_Update->Dog_Mpu_Data->pit;
    Dog_Attitude_Update->Dog_Now_Roll = Dog_Attitude_Update->Dog_Mpu_Data->rol; 
	Dog_Attitude_Update->Dog_Now_Yaw = Dog_Attitude_Update->Dog_Mpu_Data->yaw;  

	Dog_Attitude_Update->Dog_Now_Pitch_WOMpu = asin((Dog_Attitude_Update->Dog_Body_Now_Cood[0][1] + Dog_Attitude_Update->Dog_Body_Now_Cood[1][1] -\
	Dog_Attitude_Update->Dog_Body_Now_Cood[2][1] - Dog_Attitude_Update->Dog_Body_Now_Cood[3][1])/(Dog_Length)) / 3.1415925 * 180;  
	Dog_Attitude_Update->Dog_Now_Roll_WOMpu = atan((Dog_Attitude_Update->Dog_Body_Now_Cood[0][1] + Dog_Attitude_Update->Dog_Body_Now_Cood[2][1] -\
	Dog_Attitude_Update->Dog_Body_Now_Cood[1][1] - Dog_Attitude_Update->Dog_Body_Now_Cood[3][1])/(Dog_Width)) / 3.1415925 * 180;   
}




/*******************************************************************************
  * @funtion      : Lite_Dog_Attitude_To_Coordinate  
  * @LastEditors  : JackyJuu
  * @description  : 获取机器狗当前姿态转换为坐标系数据
  * @param         {Lite_Dog_Struct} *Dog_Attitude_To_Coordinate
  * @param         {float*} X
  * @param         {float*} Y
  * @param         {float*} Z
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Attitude_To_Coordinate(Lite_Dog_Struct *Dog_Attitude_To_Coordinate, float* X, float* Y, float* Z)
{
	float length = Dog_Attitude_To_Coordinate->Dog_Config_Data.body_length;
	float width = Dog_Attitude_To_Coordinate->Dog_Config_Data.body_width;

	float roll = Dog_Attitude_To_Coordinate->Target_Attitude.roll * PI / 180.00f; // 俯仰角
	float pitch = Dog_Attitude_To_Coordinate->Target_Attitude.pitch * PI / 180.00f; // 滚转角
	float yaw = 0.00f;
	// float yaw = Dog_Attitude_To_Coordinate->Dog_State_Data.Dog_Now_Yaw * PI / 180.00f; // 航向角

	float Set_X = Dog_Attitude_To_Coordinate->Target_Attitude.x;
	float Set_Y = Dog_Attitude_To_Coordinate->Target_Attitude.y;
	float Set_Z = Dog_Attitude_To_Coordinate->Target_Attitude.z;


	Y[0] = length/2 - Set_X - (length*cos(pitch)*cos(yaw))/2 + (width*cos(pitch)*sin(yaw))/2;
	Z[0] = -Set_Z - (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;

	Y[1] = length/2 - Set_X - (length*cos(pitch)*cos(yaw))/2 - (width*cos(pitch)*sin(yaw))/2;
	Z[1] = (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - Set_Z - (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;

	Y[2] = (length*cos(pitch)*cos(yaw))/2 - Set_X - length/2 + (width*cos(pitch)*sin(yaw))/2;
	Z[2] = -Set_Z - (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 + (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;

	Y[3] = (length*cos(pitch)*cos(yaw))/2 - Set_X - length/2 - (width*cos(pitch)*sin(yaw))/2;
	Z[3] = (width*(cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)))/2 - Set_Z + (length*(sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch)))/2;
}


//倒地自恢复
/*******************************************************************************
  * @funtion      : Dog_Recovery_Mode  
  * @LastEditors  : JackyJuu
  * @description  : 倒地自恢复
  * @param         {Lite_Dog_Struct*} Dog_Recovery 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Dog_Recovery_Event(Lite_Dog_Struct* Dog_Recovery)
{
	static Dog_Body_Now_Attitude_Enum Dog_Body_Last_Att;
	static int State_Time_Count[3] = {0};
	float Get_Pit,Get_Roll;
	Get_Pit = Dog_Recovery->Dog_State_Data.Dog_Now_Pitch - Dog_Recovery->Dog_Config_Data.pitch_error;
	Get_Roll = Dog_Recovery->Dog_State_Data.Dog_Now_Roll - Dog_Recovery->Dog_Config_Data.roll_error;

	if(Get_Pit < -70.00f)
	{
		State_Time_Count[0]++;
		State_Time_Count[1] = 0;
		State_Time_Count[2] = 0;
		if(State_Time_Count[0] >= 1000)
		{
			State_Time_Count[0] = 1000;
			Dog_Recovery->Dog_State_Data.Dog_Body_Now_Att = Body_Right_Down;
		}	
	}
	else if(Get_Pit > 70.00f)
	{
		State_Time_Count[0] = 0;
		State_Time_Count[1]++;
		State_Time_Count[2] = 0;
		if(State_Time_Count[1] >= 1000)
		{
			State_Time_Count[1] = 1000;
			Dog_Recovery->Dog_State_Data.Dog_Body_Now_Att = Body_Left_Down;
		}	
	}
	else if((Get_Roll < -160.00f) || (Get_Roll > 160.00f) || (Get_Pit > 160.00f))
	{
		State_Time_Count[0] = 0;
		State_Time_Count[1] = 0;
		State_Time_Count[2]++;
			
		if(State_Time_Count[2] >= 1000)
		{
			State_Time_Count[2] = 1000;
			Dog_Recovery->Dog_State_Data.Dog_Body_Now_Att = Body_All_Down;
		}	
	}
	else
	{
		State_Time_Count[0] = 0;
		State_Time_Count[1] = 0;
		State_Time_Count[2] = 0;
		Dog_Recovery->Dog_State_Data.Dog_Body_Now_Att = Body_All_Up;
	}
	Dog_Body_Last_Att = Dog_Recovery->Dog_State_Data.Dog_Body_Now_Att;	
	
}


/*******************************************************************************
  * @funtion      : Lite_Dog_Set_Coordinate  
  * @LastEditors  : JackyJuu
  * @description  : 设置机器狗足端坐标
  * @param         {Lite_Dog_Struct*} Dog_Coordinate_To_Angle 机器狗数据结构体地址
  * @param         {float*} X X轴数据，包括四条腿的X足数据
  * @param         {float*} Y Y轴数据，包括四条腿的Y足数据
  * @param         {float*} Z Z轴数据，包括四条腿的Z足数据
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Set_Coordinate(Lite_Dog_Struct* Dog_Coordinate_To_Angle,float* X,float* Y,float* Z)
{
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][0] = X[0];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][1] = X[1];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][2] = X[2];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][3] = X[3];

	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][0] = Y[0];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][1] = Y[1];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][2] = Y[2];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][3] = Y[3];

	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][0] = Z[0];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][1] = Z[1];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][2] = Z[2];
	Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][3] = Z[3];
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Get_Coordinate  
  * @LastEditors  : JackyJuu
  * @description  : 获取机器狗足端坐标
  * @param         {Lite_Dog_Struct*} Dog_Coordinate_To_Angle 机器狗数据结构体地址
  * @param         {float*} X 保存X轴数据地址，包括四条腿的X足数据
  * @param         {float*} Y 保存Y轴数据地址，包括四条腿的Y足数据
  * @param         {float*} Z 保存Z轴数据地址，包括四条腿的Z足数据
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Get_Coordinate(Lite_Dog_Struct* Dog_Coordinate_To_Angle,float* X,float* Y,float* Z)
{
	X[0] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][0];
	X[1] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][1];
	X[2] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][2];
	X[3] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[0][3];

	Y[0] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][0];
	Y[1] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][1];
	Y[2] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][2];
	Y[3] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[1][3];

	Z[0] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][0];
	Z[1] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][1];
	Z[2] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][2];
	Z[3] = Dog_Coordinate_To_Angle->Dog_Set_XY_Coordinate[2][3];
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Set_PowerLess  
  * @LastEditors  : JackyJuu
  * @description  : 设置机器狗变软软软软
  * @param         {*}
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Set_PowerLess(void)
{
	uint8_t Servo_ID[12];
	for(int i = 0;i < 12;i++)
	{
		Servo_ID[i] = i+1;
	}
	Servo_Sync_Write_Load(Servo_ID, 12,0);
}


/*******************************************************************************
  * @funtion      : Single_Leg_Set_PowerLess  
  * @LastEditors  : JackyJuu
  * @description  : 单腿设置是否产生力矩
  * @param         {Dog_Leg_Data_Struct*} Leg_Set_PowerLess 设置腿部数据结构体地址
  * @param         {uint8_t} IFload 设置是否产生力矩，1产生，0失去力矩
  * @return        {*}
  *******************************************************************************/
void Single_Leg_Set_PowerLess(Dog_Leg_Data_Struct* Leg_Set_PowerLess,uint8_t IFload)
{
	uint8_t Servo_ID[3] = {Leg_Set_PowerLess->Leg_Motor_Message[0]->id,\
	Leg_Set_PowerLess->Leg_Motor_Message[1]->id,Leg_Set_PowerLess->Leg_Motor_Message[2]->id};
	Servo_Sync_Write_Load(Servo_ID, 3, IFload);
}


/*******************************************************************************
  * @funtion      : Single_Leg_Coordinate_To_Angle  
  * @LastEditors  : JackyJuu
  * @description  : 设置单腿坐标，解算为角度
  * @param         {float} Set_x
  * @param         {float} Set_Y
  * @param         {float} Set_Z
  * @param         {Dog_Leg_Data_Struct*} Dog_Leg_Data_Set
  * @return        {*}
  *******************************************************************************/
void Single_Leg_Coordinate_To_Angle(Dog_Leg_Data_Struct* Dog_Leg_Data_Set,float Set_x,float Set_Y,float Set_Z)
{	
	Dog_Leg_Data_Set->Set_Leg_Loc[0] = Set_x;
	Dog_Leg_Data_Set->Set_Leg_Loc[1] = Set_Y;
	Dog_Leg_Data_Set->Set_Leg_Loc[2] = Set_Z;
	Get_Dog_Legs_Angle(Set_Y,Set_Z,&Dog_Leg_Data_Set->Leg_Angle_Set[1],&Dog_Leg_Data_Set->Leg_Angle_Set[2]);
}


/*******************************************************************************
  * @funtion      : Single_Leg_Angle_To_Cood  
  * @LastEditors  : JackyJuu
  * @description  : 设置单腿角度数据，自动转化为坐标数据保存，如果只想设置其中角度另外角度值给0即可
  * @param         {float} Set_A_Angle 设置水平舵机角度，以水平为0度，向上正角度
  * @param         {float} Set_B_Angle 设置大腿舵机角度，与行进方向角度相差角度，90-270度
  * @param         {float} Set_C_Angle 设置小腿舵机角度，30-90度
  * @param         {Dog_Leg_Data_Struct*} Dog_Leg_Data_Set
  * @return        {*}
  *******************************************************************************/
void Single_Leg_Angle_To_Cood(float Set_A_Angle,float Set_B_Angle,float Set_C_Angle,Dog_Leg_Data_Struct* Dog_Leg_Data_Set)
{

	if(Set_A_Angle != 0)
	{
		Dog_Leg_Data_Set->Leg_Angle_Set[0] = Set_A_Angle/180 * PI;
	}	

	if(Set_B_Angle != 0)
	{
		if(Set_B_Angle >= 90.00f)
		{
			Set_B_Angle = 90.00f;
		}
		else if(Set_B_Angle <= 30.00f)
		{
			Set_B_Angle = 30.00f;
		}
		Dog_Leg_Data_Set->Leg_Angle_Set[1] = Set_B_Angle/180 * PI;
	}	

	if(Set_C_Angle != 0)
	{
		Dog_Leg_Data_Set->Leg_Angle_Set[2] = Set_C_Angle/180 * PI;
	}	
	Get_XY_Coordinate(Dog_Leg_Data_Set->Leg_Angle_Now[1],Dog_Leg_Data_Set->Leg_Angle_Now[2],\
					 &Dog_Leg_Data_Set->Now_Leg_Loc[1],&Dog_Leg_Data_Set->Now_Leg_Loc[2]);
}


/*******************************************************************************
  * @funtion      : Single_Leg_Data_To_Servo  
  * @LastEditors  : JackyJuu
  * @description  : 获取单腿腿部角度数据转化为舵机数据发送至舵机
  * @param         {Dog_Leg_Data_Struct*} Single_Legs_Data 单腿数据地址
  * @param         {uint16_t} Set_Time 设置移动时间，0为最大速度
  * @param         {uint16_t} Set_Speed 设置移动速度，0为最大速度
  * @return        {*}
  *******************************************************************************/
void Single_Leg_Data_To_Servo(Dog_Leg_Data_Struct* Single_Legs_Data,uint16_t Set_Time,uint16_t Set_Speed)
{
	uint8_t Servo_ID[3];
	uint16_t Servo_Position[3];
	uint16_t Servo_Speed[3] = {Set_Speed,Set_Speed,Set_Speed};
	uint16_t Servo_Time[3] = {Set_Time,Set_Time,Set_Time};
	int servo_num = 3;
	float Get_B_Angle[4];

	switch(Single_Legs_Data->Leg_Loc)
	{
		case Dog_LF_Leg:
			Single_Legs_Data->Leg_Servo_Pos_Set[0] = Single_Legs_Data->Leg_Offset_Data[0];
			Single_Legs_Data->Leg_Servo_Pos_Set[1] = Single_Legs_Data->Leg_Offset_Data[1] + (180 - (Single_Legs_Data->Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
			Single_Legs_Data->Leg_Servo_Pos_Set[2] = Single_Legs_Data->Leg_Servo_Pos_Set[1] - (Single_Legs_Data->Leg_Offset_Data[2] + ((Single_Legs_Data->Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));
		break;
		case Dog_RF_Leg:
			Single_Legs_Data->Leg_Servo_Pos_Set[0] = Single_Legs_Data->Leg_Offset_Data[0];
			Single_Legs_Data->Leg_Servo_Pos_Set[1] = Single_Legs_Data->Leg_Offset_Data[1] - (180 - (Single_Legs_Data->Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
			Single_Legs_Data->Leg_Servo_Pos_Set[2] = Single_Legs_Data->Leg_Servo_Pos_Set[1] - (Single_Legs_Data->Leg_Offset_Data[2] - ((Single_Legs_Data->Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));
		break;
		case Dog_LB_Leg:
			Single_Legs_Data->Leg_Servo_Pos_Set[0] = Single_Legs_Data->Leg_Offset_Data[0];
			Single_Legs_Data->Leg_Servo_Pos_Set[1] = Single_Legs_Data->Leg_Offset_Data[1] + (180 - (Single_Legs_Data->Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
			Single_Legs_Data->Leg_Servo_Pos_Set[2] = Single_Legs_Data->Leg_Servo_Pos_Set[1] - (Single_Legs_Data->Leg_Offset_Data[2] + ((Single_Legs_Data->Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));
		break;
		case Dog_RB_Leg:
			Single_Legs_Data->Leg_Servo_Pos_Set[0] = Single_Legs_Data->Leg_Offset_Data[0];
			Single_Legs_Data->Leg_Servo_Pos_Set[1] = Single_Legs_Data->Leg_Offset_Data[1] - (180 - (Single_Legs_Data->Leg_Angle_Set[1]/PI*180)) * SERVO_SCS_ANGLE_CHANGE;
			Single_Legs_Data->Leg_Servo_Pos_Set[2] = Single_Legs_Data->Leg_Servo_Pos_Set[1] - (Single_Legs_Data->Leg_Offset_Data[2] - ((Single_Legs_Data->Leg_Angle_Set[2]/PI*180 - Dog_C_Angle_Min) * Dog_C_Leg_Servo_Count * SERVO_SCS_ANGLE_CHANGE));
			break;
	}

	Servo_ID[0] = Single_Legs_Data->Leg_Motor_Message[0]->id;
	Servo_ID[1] = Single_Legs_Data->Leg_Motor_Message[1]->id;
	Servo_ID[2] = Single_Legs_Data->Leg_Motor_Message[2]->id;


	Servo_Position[0] = Single_Legs_Data->Leg_Servo_Pos_Set[0];
	Servo_Position[1] = Single_Legs_Data->Leg_Servo_Pos_Set[1];
	Servo_Position[2] = Single_Legs_Data->Leg_Servo_Pos_Set[2];

	Servo_Sync_Write_Position(Servo_ID, servo_num, Servo_Position, Servo_Time, Servo_Speed);
}

/*******************************************************************************
  * @funtion      : Single_Leg_Get_Servo_Data  
  * @LastEditors  : JackyJuu
  * @description  : 单腿获取舵机数据
  * @param         {Dog_Leg_Data_Struct*} Dog_Leg_Data_Get
  * @return        {*}
  *******************************************************************************/
void Single_Leg_Get_Servo_Data(Dog_Leg_Data_Struct* Dog_Leg_Data_Get)
{
	Servo_Read_Position(Dog_Leg_Data_Get->Leg_Motor_Message[0]->id);
	Servo_Read_Position(Dog_Leg_Data_Get->Leg_Motor_Message[1]->id);
	Servo_Read_Position(Dog_Leg_Data_Get->Leg_Motor_Message[2]->id);

	Servo_Read_Temperature(Dog_Leg_Data_Get->Leg_Motor_Message[0]->id);
	Servo_Read_Temperature(Dog_Leg_Data_Get->Leg_Motor_Message[1]->id);
	Servo_Read_Temperature(Dog_Leg_Data_Get->Leg_Motor_Message[2]->id);

	Servo_Read_IFLoad(Dog_Leg_Data_Get->Leg_Motor_Message[0]->id);
	Servo_Read_IFLoad(Dog_Leg_Data_Get->Leg_Motor_Message[1]->id);
	Servo_Read_IFLoad(Dog_Leg_Data_Get->Leg_Motor_Message[2]->id);
}

#define Dog_Data_Flash_Address 0x08108888
/*******************************************************************************
  * @funtion      : Lite_Dog_Write_Flash_Data  
  * @LastEditors  : JackyJuu
  * @description  : 写入机器狗Flash数据
  * @param         {Lite_Dog_Struct*} Dog_Write_Data 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
int Lite_Dog_Write_Flash_Data(Lite_Dog_Struct* Dog_Write_Data)
{
	int Dog_Data_Get[12];
	uint8_t Dog_Data_Flash[24];
	int Get_Result;

	Dog_Data_Get[0] = Dog_Write_Data->Dog_LF_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_Get[1] = Dog_Write_Data->Dog_RF_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_Get[2] = Dog_Write_Data->Dog_LB_Leg_Data.Leg_Offset_Data[0];
	Dog_Data_Get[3] = Dog_Write_Data->Dog_RB_Leg_Data.Leg_Offset_Data[0];

	Dog_Data_Get[4] = Dog_Write_Data->Dog_LF_Leg_Data.Leg_Offset_Data[1];
	Dog_Data_Get[5] = Dog_Write_Data->Dog_RF_Leg_Data.Leg_Offset_Data[1];
	Dog_Data_Get[6] = Dog_Write_Data->Dog_LB_Leg_Data.Leg_Offset_Data[1];
	Dog_Data_Get[7] = Dog_Write_Data->Dog_RB_Leg_Data.Leg_Offset_Data[1];

	Dog_Data_Get[8] = Dog_Write_Data->Dog_LF_Leg_Data.Leg_Offset_Data[2];
	Dog_Data_Get[9] = Dog_Write_Data->Dog_RF_Leg_Data.Leg_Offset_Data[2];
	Dog_Data_Get[10] = Dog_Write_Data->Dog_LB_Leg_Data.Leg_Offset_Data[2];
	Dog_Data_Get[11] = Dog_Write_Data->Dog_RB_Leg_Data.Leg_Offset_Data[2];

	for(int i = 0;i < 12;i++)
	{
		Dog_Data_Flash[i*2] = Dog_Data_Get[i] >> 8;
		Dog_Data_Flash[i*2 + 1] = Dog_Data_Get[i] &0xff;
	}

	Get_Result = Flash_Write(Dog_Data_Flash_Address,Dog_Data_Flash,24);
	return Get_Result;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Read_Flash_Data  
  * @LastEditors  : JackyJuu
  * @description  : 读取机器狗Flash数据
  * @param         {Lite_Dog_Struct*} Dog_Read_Data 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Read_Flash_Data(Lite_Dog_Struct* Dog_Read_Data)
{
	int Dog_Data_Get[12];
	uint8_t Dog_Data_Flash[24];
	Flash_Read(Dog_Data_Flash_Address,Dog_Data_Flash,24);
	for(int i = 0;i < 12;i++)
	{
		Dog_Data_Get[i] = (Dog_Data_Flash[i*2] << 8) | (Dog_Data_Flash[i*2+1]); 
		if(Dog_Data_Get[i] > 60000)
		{
			Dog_Data_Get[i] = Dog_Data_Get[i] - 65536;
		}
	}

	Dog_Read_Data->Dog_LF_Leg_Data.Leg_Offset_Data[0] = Dog_Data_Get[0];
	Dog_Read_Data->Dog_RF_Leg_Data.Leg_Offset_Data[0] = Dog_Data_Get[1];
	Dog_Read_Data->Dog_LB_Leg_Data.Leg_Offset_Data[0] = Dog_Data_Get[2];
	Dog_Read_Data->Dog_RB_Leg_Data.Leg_Offset_Data[0] = Dog_Data_Get[3];
	Dog_Read_Data->Dog_LF_Leg_Data.Leg_Offset_Data[1] = Dog_Data_Get[4];
	Dog_Read_Data->Dog_RF_Leg_Data.Leg_Offset_Data[1] = Dog_Data_Get[5];
	Dog_Read_Data->Dog_LB_Leg_Data.Leg_Offset_Data[1] = Dog_Data_Get[6];
	Dog_Read_Data->Dog_RB_Leg_Data.Leg_Offset_Data[1] = Dog_Data_Get[7];
	Dog_Read_Data->Dog_LF_Leg_Data.Leg_Offset_Data[2] = Dog_Data_Get[8];
	Dog_Read_Data->Dog_RF_Leg_Data.Leg_Offset_Data[2] = Dog_Data_Get[9];
	Dog_Read_Data->Dog_LB_Leg_Data.Leg_Offset_Data[2] = Dog_Data_Get[10];
	Dog_Read_Data->Dog_RB_Leg_Data.Leg_Offset_Data[2] = Dog_Data_Get[11];
}

